/*
 * @file position_target_server.cpp
 *
 * @brief the robot will search for a ball near the robot in this state, reject balls that are far away.
 * @author Michiel Hegemans
 * @author Peter Evers
 *
 * @date December 2012
 */

#include "servers/position_target_server.h"

/**
 * Callback receiving the markers.
 */
void PositionTargetAction::markerCallback(const ar_pose::ARMarkers::ConstPtr &msg) {
	marker_found_ = false;

	int size = msg->markers.size();
	for (int i = 0; i < size; i++) {
		ar_pose::ARMarker marker = msg->markers[i];
		int id = marker.id;
		if (id == marker_search_id_ && marker.pose.pose.position.z < 0.71) {
			if (!(marker_found_)) {
				closest_marker_ = marker;
				marker_found_ = true;
			}

			// check if this is still the closest marker
			double nz	= marker.pose.pose.position.z;
			double cz	= closest_marker_.pose.pose.position.z;

			if (nz < cz) {
				closest_marker_ = marker;
			}

			// When no marker is found, this time remains until a new marker is found.
			marker_lost_time_ = ros::Time::now();
		}
	}
}

/**
 * Constructor
 */
PositionTargetAction::PositionTargetAction(std::string name) : BaseServer(name),
	as_(nh_, name, boost::bind(&PositionTargetAction::execute, this, _1), false)
{
	marker_found_ = false;
	as_.start();
	init();
	marker_lost_time_ = ros::Time::now();
}

/**
 * Destructor.
 */
PositionTargetAction::~PositionTargetAction() {
}

/**
 * Subscribe to topics and get the trunk parameter.
 */
void PositionTargetAction::init() {
	marker_sub_ = nh_.subscribe<ar_pose::ARMarkers>(Topic::AR_POSE, 1, &PositionTargetAction::markerCallback, this);
	ROS_ASSERT(nh_.getParam("trunk_distance", trunk_distance_));
	ROS_ASSERT(nh_.getParam("stop_distance", stop_distance_));
	ROS_ASSERT(nh_.getParam("threshold_x", thresholdx_));
	ROS_ASSERT(nh_.getParam("threshold_z", thresholdz_));
}

/**
 * Execute goal, position robot.
 * @param	goal	goal containing the marker to position the robot.
 */
void PositionTargetAction::execute(const rp_states::PositionTargetGoalConstPtr &goal) {
	marker_search_id_ = goal->marker_id;

	ROS_INFO("Positioning, moving trunk down.");
	comm::trunk msg;
	msg.state = TRUNK_NEUTRAL;
	publishTrunk(msg);
	ros::Duration(1).sleep();

	result_.at_location = false;
	if (!marker_found_) {
		ROS_INFO("No markers found!");
		as_.setAborted(result_);
		return;
	}

	double x = closest_marker_.pose.pose.position.x;
	double z = closest_marker_.pose.pose.position.z;

	int time_diff = ros::Time::now().sec - marker_lost_time_.sec;

	while (((fabs(x) > thresholdx_) || (fabs(z - trunk_distance_ - stop_distance_) > thresholdz_)) && ((time_diff < 1) || marker_found_)) {
		x = closest_marker_.pose.pose.position.x;
		z = closest_marker_.pose.pose.position.z;

		// TODO in position failure?
		geometry_msgs::Twist twist;
		// Create new message, else the header indicates incorrect time. TODO: Not neccesa
		// lol frame of reference.
		twist.linear.x	= z - trunk_distance_ - stop_distance_;
		twist.angular.z	= -x;

		publishWheelsSpeed(twist);
		time_diff = ros::Time::now().sec - marker_lost_time_.sec;
	}

	// Create new message, else the header indicates incorrect time. TODO: Not needed.
	geometry_msgs::Twist twist;
	twist.linear.x	= 0.0;
	twist.angular.z = 0.0;

	publishWheelsSpeed(twist);

	result_.at_location = marker_found_;
	as_.setSucceeded(result_);
}

/**
 * Get the closest marker.
 */
ar_pose::ARMarker PositionTargetAction::getClosestMarker()
{
	return closest_marker_;
}

/**
 * Set the closest marker (for testing purpose).
 * @param	marker	the marker.
 */
void PositionTargetAction::setClosestMarker(ar_pose::ARMarker marker)
{
	closest_marker_ = marker;
}

/**
 * Check if a marker is found.
 */
bool PositionTargetAction::isMarkerFound()
{
	return marker_found_;
}

/**
 * Set the found state of the marker (for testing purpose).
 * @param	found	boolean with the state of the marker (true is found, false is not found duh!).
 */
void PositionTargetAction::setMarkerFound(bool state)
{
	marker_found_ = state;
}




